#ifndef __map_mapper_h__
#define __map_mapper_h__

#include <ros/ros.h>

#include "map/occupancy_grid.h"

class Mapper
{
public:
	Mapper();

	void render();
	void addSensorRay(vec2 from, vec2 to, bool hit);
	void addSensorRay(vec2 origin, float direction, float mean, float std_dev, float height);
	float probabilityOfSensorRay(vec2 origin, float direction, float mean, float std_dev, float height);
	void addHitWithDistribution(vec2 location, float std_dev, float height);
	void subtractRobotArea(vec2 location, float std_dev, float height);

	OccupancyGrid* getOccupancyGrid() { return &occupancy_grid_; }
private:
	void addOccupancyGrid(std::vector<int8_t>& map_data);
	void render(std::vector<int8_t>& map_data);

	OccupancyGrid occupancy_grid_;

	ros::Publisher map_publisher_;

	// Holds temporary data for the map rendering
	std::vector<int8_t> map_data;
};

#endif
